 
 
//
// Created by sukai on 22-9-20.
// 发布机器人当前坐标
//
#include <iostream>
#include <ros/ros.h>
#include <ros/package.h>
#include <signal.h>
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose2D.h>
 
 
using namespace std;
 
bool is_stamped =false;//is_stamped
 
void mySigintHandler(int sig)
{
    // Do some custom action.
    // For example, publish a stop message to some other nodes.
 
    // All the default sigint handler does is call shutdown()
    ros::shutdown();
}
 
 
// rosrun contnav    currency_robotpose_node
int main(int argc,char *argv[]){
 
    setlocale(LC_ALL,"");  //中文
    //节点名
    string nodeName = "currency_robotpose_node";
    //初始化节点
    ros::init(argc,argv,nodeName);
    //创建节点
    ros::NodeHandle node;
 
    signal(SIGINT, mySigintHandler);
    //topic名字  外参标定时发送小车实时pose
    string topicName_odom = "/robot_pose_me";
    ros::Publisher   p_pub,pose2d_pub;
    if(is_stamped)
        p_pub = node.advertise<geometry_msgs::PoseStamped>(topicName_odom, 1);
    else
        p_pub = node.advertise<geometry_msgs::Pose>(topicName_odom, 1);//js -> geometry_msgs/Pose
 
    pose2d_pub = node.advertise<geometry_msgs::Pose2D>("/robot_pose2d", 1);//js -> geometry_msgs/Pose
    tf::TransformListener listener;
    listener.waitForTransform("/map", "/base_link", ros::Time(), ros::Duration(1.0));
    ros::Rate rate(300);
    while (ros::ok()) {
        tf::StampedTransform transform;
        try {
            //监听小车在基坐标下的相对坐标 /slamware_map  /map
            // listener.lookupTransform("/odom", "/base_link", ros::Time(0), transform);
            listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
            /**
             * 转欧拉角
            const tf::Quaternion &quaternion = transform.getRotation();
            double roll, pitch, yaw;//定义存储r\p\y的容器
            tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);//进行转换
            */
            //=======================================
 
            geometry_msgs::PoseStamped pose_stamped;
            pose_stamped.header.frame_id = "/map";
            pose_stamped.header.stamp = ros::Time::now();
 
            pose_stamped.pose.orientation.x = transform.getRotation().getX();
            pose_stamped.pose.orientation.y = transform.getRotation().getY();
            pose_stamped.pose.orientation.z = transform.getRotation().getZ();
            pose_stamped.pose.orientation.w = transform.getRotation().getW();
 
            pose_stamped.pose.position.x = transform.getOrigin().getX();
            pose_stamped.pose.position.y = transform.getOrigin().getY();
            pose_stamped.pose.position.z = transform.getOrigin().getZ();
 
 
            if(is_stamped)
                p_pub.publish(pose_stamped);
            else
                p_pub.publish(pose_stamped.pose);
 
 
            //转 pose2D
            double roll,pitch,yaw;
            geometry_msgs::Pose2D pose2D;
            tf::Matrix3x3(transform.getRotation()).getRPY(roll,pitch,yaw);
            pose2D.x=transform.getOrigin().getX();
            pose2D.y=transform.getOrigin().getY();
            pose2D.theta=yaw;
            pose2d_pub.publish(pose2D);
 
        } catch (exception e) {
            //     ROS_INFO_STREAM(e.what());
            rate.sleep();
            continue;
        }
 
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}